# -*- coding: utf-8 -*-
"""
Created on Tue Apr  5 19:17:56 2022

@author: lizhuang
"""
import numpy as np
import numpy.matlib
sin=np.sin;cos=np.cos
rc1=0.2
l=0.4
l2=1.2
rc2=0.6
rp2=0.3
g=9.8
m1=1.2
m2=3.6
m3=6
fx2=10
fy2=10
fx3=20
mm1=25
x=5
y=10
等效力矩对比=mm1+fx2*l*np.sin(x)+fy2*l*np.cos(y)-fx3*l*np.sin(x)-(m1*rc1+m2*l+m3*l)*g*np.cos(x)+ \
(fx2*rp2*np.sin(y)+fy2*rp2*np.cos(y)-fx3*l2*np.sin(y)+((m2*rc2+m3*l2)*g*np.cos(y)))*(l*np.cos(x)/l2*np.cos(y))

结果=-0.333333333333333*(-58.8j*(-1.2*sin(y) + 1.2j*cos(y)) - 35.28j*(-0.6*sin(y) + 0.6j*cos(y)))*cos(x)*cos(y) \
    -0.333333333333333*(20.0j*(-1.2*sin(y) + 1.2j*cos(y)) + (10.0 - 10.0j)*(-0.3*sin(y) + 0.3j*cos(y)))*cos(x)*cos(y) \
        - 74.08j*(-0.4*sin(x) + 0.4j*cos(x)) + (10.0 - 10.0j)*(-0.4*sin(x) + 0.4j*cos(x)) - 11.76j*(-0.2*sin(x) + 0.2j*cos(x))  \
            + 25
质心雅克比=np.mat([[-0.2*sin(x) + 0.2j*cos(x), 0], [-0.4*sin(x) + 0.4j*cos(x), -0.6*sin(y) + 0.6j*cos(y)],  \
        [-0.4*sin(x) + 0.4j*cos(x), -1.2*sin(y) + 1.2j*cos(y)]])
受力雅克比=np.mat([[-0.4*sin(x) + 0.4j*cos(x), -0.3*sin(y) + 0.3j*cos(y)], \
        [-0.4*sin(x) + 0.4j*cos(x), -1.2*sin(y) + 1.2j*cos(y)]])
mg=np.mat([[-m1*g*1j,-m2*g*1j,-m3*g*1j]])
f=np.mat([-fx2*1j+fy2,fx3*1j])
d=-(l*cos(x)/l2*cos(y))
dmn=np.mat([[1],[d]])
力矩=np.matmul(np.matmul(mg,质心雅克比),dmn)+np.matmul(np.matmul(f,受力雅克比),dmn)+mm1
#mg.vdot(质心雅克比).dot(dmn)
解=-0.333333333333333*(-21.0*sin(y) + 3.0*cos(y))*cos(x)*cos(y) - 4.0*sin(x) + 30.576*cos(x)*cos(y)**2 - 35.984*cos(x) + 25